using System;
using System.Collections.Generic;
using System.Text;

namespace CommonTypes
{
    public class Quaternion
    {
        const double EPSILON = 0.005f;		// error tolerance for check
        double x, y, z, w;

        public Quaternion()
        {
        }

        public Quaternion(double x, double y, double z, double w)
        {
            this.x = x;
            this.y = y;
            this.z = z;

            this.w = w;

            Normalize();

        }

        public void EulerToQuat(double x, double y, double z)
        {
            double ex, ey, ez;		// temp half euler angles
            double cr, cp, cy, sr, sp, sy, cpcy, spsy;		// temp vars in roll,pitch yaw

            ex = Geometry.DegToRad(x) / 2.0;	// convert to rads and half them
            ey = Geometry.DegToRad(y) / 2.0;
            ez = Geometry.DegToRad(z) / 2.0;

            cr = System.Math.Cos(ex);
            cp = System.Math.Cos(ey);
            cy = System.Math.Cos(ez);

            sr = System.Math.Sin(ex);
            sp = System.Math.Sin(ey);
            sy = System.Math.Sin(ez);

            cpcy = cp * cy;
            spsy = sp * sy;

            this.w = (cr * cpcy + sr * spsy);

            this.x = (sr * cpcy - cr * spsy);
            this.y = (cr * sp * cy + sr * cp * sy);
            this.z = (cr * cp * sy - sr * sp * cy);

            Normalize();

            // end void EulerToQuat(..)
        }


        public void GetAxisAngle(out  double x1, out double y1, out double z1, out double angle)
        {
            x1 = y1 = z1 = angle = 0.0;

            double temp_angle;		// temp angle
            double scale;			// temp vars

            temp_angle = System.Math.Acos(w);

            scale = System.Math.Sqrt(x * x + y * y + z * z);

            if (scale == 0.0)		// angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0
            {
                angle = 0.0;

                x1 = 0.0f;
                y1 = 0.0f;
                z1 = 1.0f;		// any axis will do
            }
            else
            {
                angle = (temp_angle * 2.0);		// angle in radians

                angle = Geometry.RadToDeg(angle);

                x1 = x / scale;
                y1 = y / scale;
                z1 = z / scale;

                double len = System.Math.Sqrt(x1 * x1 + y1 * y1 + z1 * z1);

                if (len == 0.0)
                    x1 = y1 = z1 = 0.0;
                else
                {
                    x1 /= len;
                    y1 /= len;
                    z1 /= len;
                }

            }

        }

        public void GetAxisAngle(out  double x1, out double y1, out double z1, out double angle, double angleX, double angleY, double angleZ)
        {
            x1 = y1 = z1 = angle = 0.0;

            double cx = System.Math.Cos(Geometry.DegToRad(angleX));
            double sx = System.Math.Sin(Geometry.DegToRad(angleX));
            double cy = System.Math.Cos(Geometry.DegToRad(angleY));
            double sy = System.Math.Sin(Geometry.DegToRad(angleY));
            double cz = System.Math.Cos(Geometry.DegToRad(angleZ));
            double sz = System.Math.Sin(Geometry.DegToRad(angleZ));


            //ANGLE//
            double tempAngle = (cy * cz + cz * cx + sx * sz * sy + cx * cy - 1) / 2;
            double angleR = System.Math.Acos(tempAngle);
            angle = Geometry.RadToDeg(angleR);


            ///VECTOR
            x1 = (cy * sx + sx * cz - sz * sy * cx) / 2 * System.Math.Sin(angleR);
            y1 = (sx * sz + sy * cz * cx + sy) / 2 * System.Math.Sin(angleR);
            z1 = (sz * cy + cx * sz - sy * cz * sx) / 2 * System.Math.Sin(angleR);


        }


        public void AxisToQuat(double xv, double yv, double zv, double angle)
        {
            double rad, scale;		// temp vars

            if (xv == 0.0f && yv == 0.0f && zv == 0.0f)			// if axis is zero, then return quaternion (1,0,0,0)
            {
                this.w = 1.0;
                this.x = 0.0;
                this.y = 0.0;
                this.z = 0.0;
            }


            rad = Geometry.DegToRad(angle) / 2;

            this.w = System.Math.Cos(rad);

            scale = System.Math.Sin(rad);

            this.x = (xv * scale);
            this.y = (yv * scale);
            this.z = (zv * scale);

            Normalize();		// make sure a unit quaternion turns up

        }	// end 

        public static Quaternion operator *(Quaternion q1, Quaternion q2)
        {
            double rx, ry, rz, rw;		// temp result

            rw = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;

            rx = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
            ry = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
            rz = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;

            return new Quaternion(rx, ry, rz, rw);
        }

        public static Quaternion operator +(Quaternion q1, Quaternion q2)
        {
            double rx, ry, rz, rw;		// temp result

            rw = q1.w + q2.w;

            rx = q1.x + q2.x;
            ry = q1.y + q2.y;
            rz = q1.z + q2.z;

            return new Quaternion(rx, ry, rz, rw);
        }

        private void Normalize()
        {

            ZeroClamp();

            double norm = System.Math.Sqrt(x * x + y * y + z * z + w * w);

            if (norm == 0)
            {
                x = y = z = 0;
                w = 1;

            }
            else
            {
                x = (x / norm);
                y = (y / norm);
                z = (z / norm);
                w = (w / norm);
            }

            LIMIT_RANGE(-1.0f, ref w, 1.0);

            LIMIT_RANGE(-1.0f, ref x, 1.0);
            LIMIT_RANGE(-1.0f, ref y, 1.0);
            LIMIT_RANGE(-1.0f, ref z, 1.0);

        }

        private void LIMIT_RANGE(double low, ref double Value, double high)
        {
            if (Value < low)
                Value = low;
            else if (Value > high)
                Value = high;
        }

        private void ZeroClamp()
        {
            ZERO_CLAMP(ref x);
            ZERO_CLAMP(ref y);
            ZERO_CLAMP(ref z);

            ZERO_CLAMP(ref w);
        }	// end void Zero_Cla

        private void ZERO_CLAMP(ref double x)
        {
            if (EPSILON > System.Math.Abs(x))// set double to 0 if within tolerance
                x = 0.0;
        }


        public static void GetMultipleRotation(out  double x1, out double y1, out double z1, out double angle, double angleX, double angleY, double angleZ)
        {
            Quaternion qX = new Quaternion();
            Quaternion qY = new Quaternion();
            Quaternion qZ = new Quaternion();

            Quaternion qEnd = new Quaternion();

            qX.AxisToQuat(1, 0, 0, angleX);
            qY.AxisToQuat(0, 1, 0, angleY);
            qZ.AxisToQuat(0, 0, 1, angleZ);

            qEnd = qY * qX * qZ;

            qEnd.GetAxisAngle(out x1, out y1, out z1, out angle);

        }

    }
}
